#include "Tool_Filter.h"

int Middle_filter(int *temp,unsigned char *k, unsigned char num,int current)
{
	q31_t TempBuf[50];
	
	temp[*k] = current;
	(*k)++;
	if ( *k == num  ) *k = 0;

	arm_copy_q31(temp,TempBuf,num);

	bubble_sort(TempBuf,num);
	return TempBuf[(num-1)/2];
}

short Middle_filter(short *temp,unsigned char *k, unsigned char num,short current)
{
	q15_t TempBuf[50];
	temp[*k] = current;
	(*k)++;
	if ( *k == num  ) *k = 0;

	arm_copy_q15(temp,TempBuf,num);

	bubble_sort(TempBuf,num);
	
	return TempBuf[(num-1)/2];
}

float Middle_filter(float *temp,unsigned char *k, unsigned char num,float current)
{
	float32_t TempBuf[50];

	temp[*k] = current;
	(*k)++;
	if ( *k == num  ) *k = 0;

	arm_copy_f32(temp,TempBuf,num);

	bubble_sort(TempBuf,num);
	return TempBuf[(num-1)/2];
}


int Slider_Filter(int *temp,unsigned char *i, unsigned char num,int current)
{
	q31_t sum=0;

	temp[*i] = current;
	(*i)++;
	if ( *i == num  ) *i = 0;

	arm_mean_q31(temp, num, &sum);
	return sum;
}


short Slider_Filter(short *temp,unsigned char *i, unsigned char num,short current)
{
	q15_t sum=0;

	temp[*i] = current;
	(*i)++;
	if ( *i == num  ) *i = 0;

	arm_mean_q15(temp, num, &sum);
	return sum;
}
float Slider_Filter(float *temp,unsigned char *i, unsigned char num,float current)
{
	float32_t sum=0;

	temp[*i] = current;
	(*i)++;
	if ( *i == num  ) *i = 0;

	arm_mean_f32(temp, num, &sum);
	return sum;
}
float SquareRootFloat(float number)
{
         long i;
         float x, y;
         const float f = 1.5F;
         x = number * 0.5F;
         y    = number;
         i    = * ( long * ) &y;
         i    = 0x5f3759df - ( i >> 1 ); 
         y    = * ( float * ) &i;
         y    = y * ( f - ( x * y * y ) );        
         y    = y * ( f - ( x * y * y ) );      
         return number * y;
}


